/*
Main project file for CENG314 Spring 2013 PRT project

This project is divided into several files. Each group is responsible
for their own file and respective section in this file. The sections
are clearly labeled via comments just like this one. Respect other 
people's work. 

DRIVER group is responsible for this file, except for the sections
below that are reserved for other groups.
*/

//header
//iyte-prt protocol commands
//let's put these into a separate file later.

const byte PRT_SE_STDTELEM_TIME_BASED=0x09;
const byte PRT_SE_EMERGENCY=0x02;
const byte PRT_SE_PASS_GETIN=0x05;
const byte PRT_SE_PASSENGER_LIST=0x06;
const byte PRT_SE_PASS_GETOUT=0x07;
const byte PRT_SE_GO=0x03;
const byte PRT_SE_STDTELEM_EVENT_BASED=0x1B;
const byte PRT_SE_READY=0x12;
const byte PRT_SE_ASSIGN_ID=0x01;
const byte PRT_SE_RECHARGE=0x04;
const byte PRT_SE_STATUS=0x08;
const byte PRT_SE_FULLTELEM=0x0A;
const byte OK= 0x7B;

const byte SERVER_ID=0x7F;  // static server id
const byte MY_ID=0x01;      // unique for each PRT

//Message(packet) Struct
struct MESSAGE {
       byte dest;
       byte src;
       byte len;
       byte cmd;
       byte params[256];
};


//Global elements for DRIVER group should be here:
MESSAGE *inMsg;
byte sendCmd;
int DR_ID;
boolean is_registered;
int counter;
byte fullTelem[24];
byte timeTelem[7]; 
byte eventTelem[15];
byte passList[16];


//Global elements for MOTOR CONTROL group should be here:
int E1 = 5; //M1 Speed Control
int E2 = 6; //M2 Speed Control
int M1 = 4; //M1 Direction Control
int M2 = 7; //M2 Direction Control
//infrared parameters
int Infradetect; 
int Goonflag=0;
//endcoder parameters
int TravelledDistance = 0;
float Speed = 0;
#define LEFT 0
#define RIGHT 1 
long coder[2] = {0,0};
int lastSpeed[2] = {0,0};

//ultrasonic parameters
int URPWM =A4; // PWM Output 0-25000US,Every 50US represent 1cm
int URTRIG=A5; // PWM trigger pin 
unsigned int obstacleDistance=0;
uint8_t EnPwmCmd[4]={0x44,0x02,0xbb,0x01};    // distance measure command
static unsigned long timer_1 = 0; 
static unsigned long timer = 0; 
static unsigned long timer_2 = 0; 
float setPoint;
float measurement;
float error;
float output;
float slSolMotor=0;
float slSagMotor=0;
boolean emergencystop=0;

//Global elements for ROUTE PLANNING group should be here:
#define TURNLEFT 1    //Global variables for other compenents usage.
#define TURNRIGHT 0   //Directions to be followed.
#define STRAIGHT 2    //Command to pass the station 
#define OVER 3        //Command indicating the route is completed

int rpNextStation;
int rpDestinationStation;
int rpCurrentStation;
boolean rpArrivalFlag; //Flag that determines whether the car arrived to the station or not.
byte directionArray[20]; //determines all decisions of the crossroads until the destination Station

int route[20];          // The path which is determined by Optimization Group, includes the stations in order to be followed to reach rpDestinationStation

//Global elements for BATTERY MANAGEMENT group should be here:  
int bmBatteryCapacity;   //simulated battery capacity 0-100%
boolean bmBatteryAlarm;  //raised when battery is below 10%
int bmanalogInput = 1;
float bmvout = 0.0;      //Vout
float bmvin = 0.0;       //Vin for voltmeter
float bmR1 = 49700.0;    // !! resistance of R1 !!----voltmeter
float bmR2 = 4030.0;     // !! resistance of R2 !!----voltmeter
int bmled = A2;          // low battery led pin        analog input 2 oalcak
// variable to store the value 
int bmvalue = 0;        // reading Vin before calculation
float bmpercentage;
byte bmpercentageforDrive;
byte bmBatteryCapacityforDrive;

//Global elements for STD. LINE FOLLOWING group should be here:
int slSensorState1;      //Definition of line following sensors
int slSensorState2;
int slSensorState3;
int slSensorState4;
int slSensorState5;

//Global elements for COMMUNICATION group should be here:

//Global elements for DOTTED LINE FOLLOWING group should be here:
int dlSensors[5];
float dlMeasurement;
int dlCount;
float dlError;
float dlLastError;
float dlDErr;
int dlSetpoint = 8;
float dlProportional;
float dlDerivative;
float dlTurn;
int dlMp = 120;
int dlKp = 15;
int dlKd = 2;
int dlPowerLeft;
int dlPowerRight;
int dlLeft = 0;
int dlRight = 0;
unsigned long dlLastTime;
int dlSampleTime = 2000;

#define LEFT 0
#define RIGHT 1
int dlSensor = 9;
int dlLastsensorval;
int dlDCount = 0;
int dlNewX = 0;
int dlDistancebetweenline = 20;
unsigned long dlTime1 = 0;
unsigned long dlTime2 = 0;
unsigned long dlTime3 = 0;
unsigned long dlTime;
long dlCoder[2] = {
  0,0};
int dlLastSpeed[2] = {
  0,0};  

//Global elements for PASSENGER MANAGEMENT group should be here:
#define PASSENGERCAPACITY 4  //Capacity of vehicle
int pmPassengerCount;        //number of people onboard
typedef struct                  //Structure for Passenger information
  {
      int id;
      byte depStation;
      byte desStation;
     
  }  pmPassenger;

pmPassenger pmPassengerList[PASSENGERCAPACITY]; // List for information of Passengers

//const byte PRT_SE_STDTELEM_TIME_BASED=0x09;

//DRIVER group is responsible from this point on. 

void setup() 
{   
  //uyutma gerekiyor ready komutu için  
  Serial.begin(9600);
  is_registered = false;
  counter = 0;
  PWM_Mode_Setup();
  pinMode(bmanalogInput, INPUT); // battery man. group voltmeter analog pin 1
  pinMode(bmled, OUTPUT);        // battery man. group low battery led pin 13
  pmPassengerCount = 0;
  bmBatteryCapacity = 100;
  bmBatteryAlarm = false;
  rpArrivalFlag = false; //The default value of the flag which indicates that the car is on the station or not.
  pinMode(13,INPUT);  //13 is the pin number of Infrared Sensor Switch
  pinMode(12, INPUT);    //cizgi takibi sensorlerinin input olarak tanımlanması.
  pinMode(11, INPUT);
  pinMode(10, INPUT);
  pinMode(9, INPUT);
  pinMode(8, INPUT);
  
  pinMode(4, OUTPUT); //motor output
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  
  inMsg = static_cast<MESSAGE*>(malloc(sizeof(MESSAGE)));
  //attachInterrupt(LEFT, dlLwheelSpeed, FALLING);    //init the interrupt mode for the digital pin 2
  //attachInterrupt(RIGHT, dlRwheelSpeed, CHANGE);   //init the interrupt mode for the digital pin 3
}

void loop() 
{
  while(is_registered == false){
      ready();
   }
  coReceive(inMsg);
  if(inMsg!=NULL){
    drProcessCommand();
  }
  if(counter == 200){
    createTelem(1);
    coSend(PRT_SE_STDTELEM_TIME_BASED,(char*)timeTelem);
    counter=0;
  }
  if(emergencystop == true){
    coSend(PRT_SE_EMERGENCY,(char*)emergencystop);
  }
  counter++;
  

  vehicleStartMoving();
  //put every job here sequentially. some possible examples:
  //coReceive(...);
  //drProcessCommand(...);
  //coTransmit(...);
  //bmCalculateBattery(...);
  battery();// main battery function which calls bmCalculateBattery and bmRealBattery
  //rpPlanRoute();
  //...
  //stdLineFollowing();
  //dlLineFollow();
  //dlDottedLineFollow();

}

void drProcessCommand(){
      
  switch(inMsg->cmd){
    
     case PRT_SE_GO :
       rpDestinationStation = inMsg->params[0];
       createTelem(0);
       coSend(PRT_SE_STDTELEM_EVENT_BASED,(char*)eventTelem);
     break; 
     case PRT_SE_RECHARGE : 
       bmRechargeBattery();
       coSend(OK,NULL);
     break; 
     case PRT_SE_PASS_GETIN : 
        byte des,dep,pID;
        dep = inMsg->params[0];
        des = inMsg->params[1];
        pID = inMsg->params[2];
        pmPassengerGetIn(pID,dep,des);
        makeClear();
        coSend(PRT_SE_PASSENGER_LIST,(char*)passList); 
     break;  
     case PRT_SE_PASS_GETOUT : 
        byte curstation;
        curstation = inMsg->params[0];
        pmPassengerGetOut(curstation);
        makeClear();
        coSend(PRT_SE_PASSENGER_LIST,(char*)passList);
     break;     
     case PRT_SE_STATUS :
        createTelem(2);//FULL TELEM
        coSend(PRT_SE_FULLTELEM,(char*)fullTelem);
     break;  
     default:break;
  }
  
}

void createTelem(int telemType)
{
         int pass1 = pmPassengerList[0].id;
         int pass2 = pmPassengerList[1].id;
         int pass3 = pmPassengerList[2].id;
         int pass4 = pmPassengerList[3].id;
         
          //Passenger 1
         byte pass1_high = highByte(pass1);
         byte pass1_low = lowByte(pass1);
         
         //Passenger 2
         byte pass2_high = highByte(pass2);
         byte pass2_low = lowByte(pass2);
         

         //Passenger 3
         byte pass3_high = highByte(pass3);
         byte pass3_low = lowByte(pass3);
         
         //Passenger 4         
         byte pass4_high = highByte(pass4);
         byte pass4_low = lowByte(pass4);

         
         
         //TravelledDistance
         byte travel_High = highByte(TravelledDistance);
         byte travel_Low = lowByte(TravelledDistance);

         //Obstacle Distance 
         byte obstacle_High = highByte(obstacleDistance);
         byte obstacle_Low = lowByte(obstacleDistance);
    
    switch(telemType){
      case 0: 
          eventTelem[0] = pass1_high;
          eventTelem[1] = pass1_low;
          eventTelem[2] = pass2_high;
          eventTelem[3] = pass2_low;
          eventTelem[4] = pass3_high;
          eventTelem[5] = pass3_low;
          eventTelem[6] = pass4_high;
          eventTelem[7] = pass4_low;
          eventTelem[8] = emergencystop;
          eventTelem[9] = bmBatteryCapacityforDrive;
          eventTelem[10]= bmpercentageforDrive;
          eventTelem[11]= rpCurrentStation;
          eventTelem[12]= rpDestinationStation;
          eventTelem[13]= travel_High;
          eventTelem[14]= travel_Low;
        break;
      
      case 1:
          timeTelem[0]=bmBatteryCapacityforDrive;
          timeTelem[1]=bmpercentageforDrive;
          timeTelem[2]= (byte)lastSpeed[1];
          timeTelem[3]=0;//x odometri degeri olmadigi icin 0 verilmiştir
          timeTelem[4]=0;//y odometri degeri olmadigi icin 0 verilmiştir
          timeTelem[5]=travel_High;  
          timeTelem[6]=travel_Low;    

        break;
      case 2:      
          fullTelem[0] = pass1_high;
          fullTelem[1] = pass1_low;
          fullTelem[2] = pass2_high;
          fullTelem[3] = pass2_low;
          fullTelem[4] = pass3_high;
          fullTelem[5] = pass3_low;
          fullTelem[6] = pass4_high;
          fullTelem[7] = pass4_low;
          fullTelem[8] = bmBatteryCapacityforDrive;
          fullTelem[9] = bmpercentageforDrive;
          fullTelem[10] = rpCurrentStation;
          fullTelem[11] = rpDestinationStation;
          fullTelem[12] = travel_High; 
          fullTelem[13] = travel_Low; 
          fullTelem[14] = obstacle_High; 
          fullTelem[15] = obstacle_Low; 
          fullTelem[16] = (byte)lastSpeed[1];
          fullTelem[17] = (byte)slSensorState1;
          fullTelem[18] = (byte)slSensorState2;
          fullTelem[19] = (byte)slSensorState3;
          fullTelem[20] = (byte)slSensorState4;
          fullTelem[21] = (byte)slSensorState5;
          fullTelem[22] = 0;//x odometri degeri olmadigi icin 0 verilmiştir
          fullTelem[23] = 0;//y odometri degeri olmadigi icin 0 verilmiştir
        break;
        
        default: break;
}       
      //return tel_params;
}

void ready(){
  coSend(PRT_SE_READY,NULL);
  delay(2000);
  if(coReceive(inMsg)){
    if(inMsg->cmd == PRT_SE_ASSIGN_ID){
      DR_ID = (int) inMsg->params[0]; 
      coSend(OK,NULL);
      is_registered=true; 
    }
  }
}


void makeClear(){
  int i;
  int id;
  for(i=0 ; i<4 ; i++){
    passList[4*i]=highByte(pmPassengerList[i].id);
    passList[4*i+1]=lowByte(pmPassengerList[i].id);
    passList[4*i+2] = pmPassengerList[i].depStation;
    passList[4*i+3] = pmPassengerList[i].desStation;
 }
 


}


